/*
 * Copyright (c) 2015, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 *  ======== empty.c ========
 */
/* XDCtools Header files */
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <xdc/std.h>
#include <xdc/runtime/System.h>

/* BIOS Header files */
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Swi.h>
#include <ti/sysbios/knl/Semaphore.h>
/* TI-RTOS Header files */


/* Board Header file */
#include "Board.h"
#include "inc/hw_memmap.h"
#include "driverlib/pin_map.h"
#include "driverlib/sysctl.h"
#include "driverlib/timer.h"
#include "driverlib/gpio.h"
#include "driverlib/fpu.h"
#include "driverlib/pwm.h"
#include "driverlib/adc.h"
#include "driverlib/uart.h"
#include "driverlib/interrupt.h"
#include "my_config_lib.h"
#include "my_cmd_lib.h"
#include "my_funcs.h"



char cmd_received[10];
volatile bool pid_cmd = 0;
volatile bool adcDone = 1;
volatile bool PIDStart = 0;
uint32_t adcData[8];
uint32_t PingPongBuf[2][20];
volatile int pingIdx = 0;
volatile int pingDataNum = 0;

extern Semaphore_Handle semlookupcmd;
extern Semaphore_Handle semdetectLine;
extern Semaphore_Handle semADC;
extern Semaphore_Handle semPing;
extern Semaphore_Handle semPong;
extern Semaphore_Handle semRedBlink;
extern Swi_Handle UART1_Swi;

volatile uint32_t timestamp = 0; // 5 ms
volatile uint32_t last_irdTrigger = 0;
volatile uint32_t detectT;
volatile bool pingpong = 0;
volatile double error;

/*
 *  ======== main ========
 */

int main(void)
{
	FPUEnable();
	FPULazyStackingEnable();
	SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
				   SYSCTL_XTAL_16MHZ);
	MyUART_Init();
	MyADC_Init();
	MyGPIO_Init();
	MyPWM_Init();
	Mytimer_Init();
	IntMasterEnable();
    /* Start BIOS */
    BIOS_start();
    return (0);
}

void cmd_Task(void){
	UARTSend((uint8_t *)"\033[2J\033[HPlease enter 3-letter commands from the Bluetooth terminal: \r\n",
				UART1_BASE);
	while(1)
	{
		Semaphore_pend(semlookupcmd, BIOS_WAIT_FOREVER);
		int i;
		for(i = 0; i < cmd_Table.cmd_num; i++){
			if (!strcmp(cmd_received, cmd_Table.cmd_Tablearray[i].cmd_name))
			{
				cmd_Table.cmd_Tablearray[i].func();
				break;
			}
		}
		if (i == cmd_Table.cmd_num) {
			UARTSend((uint8_t *)"\r\nCMD Invalid!\r\n", UART1_BASE);
		}
	}
}

void PID_Task(void){
	static int filldata = 0;
	while(1){
		Semaphore_pend(semADC, BIOS_WAIT_FOREVER);
		double adcData_aveR = (adcData[0] + adcData[1] + adcData[2] + adcData[3]) / 4.0;
		double adcDate_aveF = (adcData[4] + adcData[5] + adcData[6] + adcData[7]) / 4.0;
		double distance_cmF = -1.084865e-9 * adcDate_aveF*adcDate_aveF*adcDate_aveF +
						0.864035e-5 * adcDate_aveF*adcDate_aveF -
						0.0254402 * adcDate_aveF + 32.40135;
		double distance_cmR = -1.328758e-9 * adcData_aveR*adcData_aveR*adcData_aveR +
						1.009207e-5 * adcData_aveR*adcData_aveR -
						0.028107 * adcData_aveR + 33.881774;
		distance_cmR = kalman_filter(distance_cmR);
		error = TARGET - distance_cmR;
		if (pingpong) {
			if (filldata) Semaphore_post(semPing);
			filldata = 1 - filldata;
		}
		if (UT || distance_cmF < uTurn_thresIn){
			robot_uturn(distance_cmF);
		}
		else if (TR || distance_cmR > rTurn_thresIn){
			robot_TR(distance_cmR);
		}
		else{
			PID_followWall(error);
		}
		adcDone = 1;
	}
}

void pong_Task(void){
	int i, tendigit, onedigit;
	char sendstr[70] = "Team11:";
	while (1){
		Semaphore_pend(semPong, BIOS_WAIT_FOREVER);
		if (pingpong) {
			for (i = 0; i < 20; i++){
				onedigit = PingPongBuf[1-pingIdx][i] % 10;
				tendigit = (PingPongBuf[1-pingIdx][i] / 10) % 10;
				sendstr[7 + i * 3] = ' ';
				sendstr[7 + i * 3 + 1] = tendigit + '0';
				sendstr[7 + i * 3 + 2] = onedigit + '0';
			}
			sendstr[7 + i * 3] = '\r';
			sendstr[7 + i * 3 + 1] = '\n';
			sendstr[7 + i * 3 + 2] = '\0';
			UARTSend((uint8_t *)sendstr, UART1_BASE);
		}
		else {
			for (i = 0; i < pingDataNum; i++){
				onedigit = PingPongBuf[pingIdx][i] % 10;
				tendigit = (PingPongBuf[pingIdx][i] / 10) % 10;
				sendstr[7 + i * 3] = ' ';
				sendstr[7 + i * 3 + 1] = tendigit + '0';
				sendstr[7 + i * 3 + 2] = onedigit + '0';
			}
			sendstr[7 + i * 3] = '\r';
			sendstr[7 + i * 3 + 1] = '\n';
			sendstr[7 + i * 3 + 2] = '\0';
			UARTSend((uint8_t *)sendstr, UART1_BASE);
		}
	}
}

void ping_Task(void){
	uint32_t abserr;
	while (1){
		Semaphore_pend(semPing, BIOS_WAIT_FOREVER);
		if (pingDataNum == 20) {
			pingIdx = 1 - pingIdx;
			pingDataNum = 0;
			Semaphore_post(semPong);
		}
		abserr = (uint32_t)abs((int)(error*10));
		if (abserr >= 100) abserr = 99;
		PingPongBuf[pingIdx][pingDataNum] = abserr;
		pingDataNum++;
	}
}

void RedBlink_Task(void){
	int duration;
	while (1){
		Semaphore_pend(semRedBlink, BIOS_WAIT_FOREVER);
		duration = 0; // 0.5 s
		GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, GPIO_PIN_1);
		while (duration < 120 && !STOP){
			Task_sleep(500);
			GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1,
					~GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_1));
			duration++;
		}
		GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, 0);
		if (!STOP) {
			STOP = 1;
			current_state = NONELINE;
		}
	}
}

void detectLine_Task(void){
	while(1){
		Semaphore_pend(semdetectLine, BIOS_WAIT_FOREVER);
		Task_sleep(2);
		if (GPIOPinRead(GPIO_PORTA_BASE, GPIO_INT_PIN_3)){
			detectT = timestamp;
			if (current_state == THINLINE2) {
				while(1) {
					if (!GPIOPinRead(GPIO_PORTA_BASE, GPIO_INT_PIN_3)) break;
					if (timestamp - detectT > thickLower) {
						current_state = THICKLINE;
						PIDStart = 0;
						PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 1);
						PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, 1);
						GPIOIntDisable(GPIO_PORTA_BASE, GPIO_INT_PIN_3);
						Semaphore_post(semRedBlink);
						break;
					}
					Task_sleep(2);
				}
			}
		}
		else {
			detectT = timestamp - detectT;
			switch (current_state) {
				case NONELINE:
					if (detectT > thinLower && detectT < thinUpper) {
						current_state = THINLINE1;
						pingpong = 1;
						GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, GPIO_PIN_3);
					}
					break;
				case THINLINE1:
					if (detectT > thinLower && detectT < thinUpper){
						current_state = THINLINE2;
						pingpong = 0;
						GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, GPIO_PIN_2);
						Semaphore_post(semPong);
					}
					break;
			}
		}
	}
}

void UART1process_Swi(void){
	UARTSend((uint8_t *)cmd_received, UART1_BASE);
	if (pid_cmd) {
		pid_cmd = 0;
	}
	else {
		Semaphore_post(semlookupcmd);
	}
}


void ADC0SS0IntHandler_Hwi(void){
	ADCIntClear(ADC0_BASE, 0);
	ADCSequenceDataGet(ADC0_BASE, 0, adcData);
	if (PIDStart) Semaphore_post(semADC);
}


void IRD_gpioAhandler_Hwi(void){
	GPIOIntClear(GPIO_PORTA_BASE, GPIO_INT_PIN_3);
	if (timestamp - last_irdTrigger > 1){
		last_irdTrigger = timestamp;
		Semaphore_post(semdetectLine);
	}
}

void UART1IntHandler_Hwi(void)
{
	UARTIntClear(UART1_BASE, UARTIntStatus(UART1_BASE, true));
	uint8_t num_chars = 0;
	char getchar;
	while(UARTCharsAvail(UART1_BASE))
	{
		getchar = (char)UARTCharGetNonBlocking(UART1_BASE);
		if (num_chars < 9){
			cmd_received[num_chars] = getchar;
			num_chars++;
		}
	}
	cmd_received[num_chars] = '\0';
	Swi_post(UART1_Swi);
}

void timer1Handler_Hwi(void){
	TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
	timestamp++;
	if (PIDStart && (timestamp % 10 == 0) && adcDone){
		adcDone = 0;
		ADCProcessorTrigger(ADC0_BASE, 0);
	}
}


